//
// Created by jg on 2020/9/8.
//


#include "pos_generator/insprobe_to_pos.h"

std::string INSPROBE_POS::double2string(const double value) {
    std::ostringstream out;
    out.precision(15);
    out << value;
    return out.str();
}

/* original_point -rad, rad, m */
void INSPROBE_POS::init(double resolution, Eigen::Vector3d original, vector<double> map_size) {
    resolution_ = resolution;

    x_min_ = map_size[0];
    x_max_ = map_size[1];
    y_min_ = map_size[2];
    y_max_ = map_size[3];

    original_point_.swap(original);
    has_original_ = true;

    pos_trans_.reset(new LOC2POS(original_point_, resolution_));
}

/* receive the location information -- insprobe_msgs */
bool INSPROBE_POS::getLocation(const insprobe_msgs::Navigation &msg, nav_msgs::Odometry &state) {
    state.header.frame_id  = "wmr";
    state.header.stamp     = msg.header.stamp;

    //Convert latitude and longitude height(BHL -deg -deg -m) to local coordinate system(xyz -m)
    if(!has_original_) return false;

    const double& vn = msg.velocity_north;
    const double& ve = msg.velocity_east;
    const double  vd = 0.0;
//   const double vd = insprobe_pack_.veldown;
    Eigen::Vector3d loc_BLH(msg.latitude * D2R, msg.longitude * D2R, msg.altitude);

    //偏航角yaw为北偏东为正，转换到局部坐标系时需要作处理
    //地图x轴正指向北、y轴正指向西
    double roll  = msg.roll  * D2R;   //deg to rad
    double pitch = msg.pitch * D2R;
    double yaw   = -msg.yaw  * D2R;
    while(yaw > 2*M_PI) yaw -= 2*M_PI;
    while(yaw < 0)      yaw += 2*M_PI;
    Eigen::Vector3d Euler(roll, pitch, yaw);  //rad

    const  short count_sum = 5;
    static bool isFirst  = true;
    static bool overMark = true;
    static short _count  = count_sum;
    if(isFirst && (!overMark && msg.weeksec != time_last_)){
        yaw_last_  = yaw;
        isFirst    = false;
        overMark   = true;
        _count     = count_sum;
        time_last_ = msg.weeksec;
    } else if(msg.weeksec == time_last_){

        if(overMark){
            ROS_WARN("[insprobe-to-pos node]: weeksec is over ");
            overMark = false;
        }

        if(_count == 0){
            isFirst = true;
            _count--;
            ROS_WARN("[insprobe-to-pos node]: no insprobe message !");
        }
        else if(_count > 0)
            _count--;

        return false;
    } else {
        if(!overMark){
            overMark = true;
            _count   = count_sum;
        }
    }

//    ROS_INFO("[pos-local]: %lf", state_.header.stamp.toSec());

    Eigen::Vector2d loc_xy;
    pos_trans_->BLH2xy(loc_BLH, loc_xy);

    state.pose.pose.position.x = loc_xy[0];
    state.pose.pose.position.y = loc_xy[1];
    state.pose.pose.position.z = 1e-5;

    Eigen::Quaterniond quan;
    OriTrans::Euler2Quater(Euler, quan);
    state.pose.pose.orientation.w = quan.w();
    state.pose.pose.orientation.x = quan.x();
    state.pose.pose.orientation.y = quan.y();
    state.pose.pose.orientation.z = quan.z();

    state.twist.twist.linear.x = vn;
    state.twist.twist.linear.y = -ve;
    state.twist.twist.linear.z = vd;   //将该项设置为0, 若赋予地速值，则可能出现由于存在z轴速度而无法规划轨迹

    //向左转角速度为正，向右为负
    double d_yaw   = yaw - yaw_last_;
    if (d_yaw > M_PI) {
        yaw_last_ += 2*M_PI;
        d_yaw      = yaw - yaw_last_;
    } else if (d_yaw < -M_PI) {
        yaw_last_ -= 2*M_PI;
        d_yaw      = yaw - yaw_last_;
    }

    double dt = msg.weeksec - time_last_;
    state.twist.twist.angular.x = 0;
    state.twist.twist.angular.y = 0;
    state.twist.twist.angular.z = (dt != 0 ? d_yaw / dt : 0.0);

    yaw_last_  = yaw;
    time_last_ = msg.weeksec;

    pos_lock_.lock();
    state_ = state;
    time_now_ = state.header.stamp;
    pos_lock_.unlock();

    return true;
}

bool INSPROBE_POS::getWayPoint(const geometry_msgs::Pose2D &msg, nav_msgs::Path& wp) {
//    if(!has_pos_) return false;
    //msg -deg, deg, m
    double target_lat = msg.x * D2R;
    double target_lon = msg.y * D2R;
    int ctrl_mode  = (int)msg.theta;  //0-handle; 1 -auto; 2 -loop

    Eigen::Vector3d target_BLH(target_lat, target_lon, original_point_[2]);
    Eigen::Vector2d target_xy;
    pos_trans_->BLH2xy(target_BLH, target_xy);
//    auto target_xy = pos_trans_->BLH2xy(target_BLH);

    geometry_msgs::PoseStamped target_point;
    static int seq_pt = 0;
    if(ctrl_mode == HANDLE || ctrl_mode == AUTO){
        wp.header.stamp     = time_now_;
        wp.header.frame_id  = (ctrl_mode == HANDLE) ?
                std::string("HANDLE") : std::string("AUTO");

        target_point.header.stamp      = time_now_;
        target_point.header.frame_id   = std::string("odom");
        if(ctrl_mode == HANDLE) seq_pt = 0;
        target_point.header.seq = seq_pt++;

        target_point.pose.position.x  = target_xy[0];
        target_point.pose.position.y  = target_xy[1];
        target_point.pose.position.z  = 0.1;

        wp.poses.push_back(target_point);
    }
    else if (ctrl_mode == LOOP){
        /* param */
    }

    return true;
}

/* receive the local pos -- xyz(m, m, m)
 * output -- BLH (deg, deg, m)
 * */
bool INSPROBE_POS::getPathPoint(const Eigen::Vector3d &pos_xyz, geometry_msgs::PoseStamped &pos_BLH) {
    if(!has_original_) return false;

    Eigen::Vector2d pxy(pos_xyz[0], pos_xyz[1]);
    Eigen::Vector3d pBLH;
    pos_trans_->XY2BLH(pxy, pBLH);

    pos_BLH.pose.position.x = pBLH[0];
    pos_BLH.pose.position.y = pBLH[1];
    pos_BLH.pose.position.z = pBLH[2];
    return true;
}

/*
void INSPROBE_POS::btraj2path(const wmr_msgs::PolynomialTrajectory& msg, path_msgs::PathMsg& path) {
    path.mode = 0;

    int time_index = 0;
    for(int i = 0; i < (int)msg.coef_x.size(); i++){
        time_index = i / (int)(msg.order.front()+1);
        Eigen::Vector2d xy(msg.coef_x[i] * msg.time[time_index] , msg.coef_y[i] * msg.time[time_index]);
        Eigen::Vector3d BLH;
        pos_trans_->XY2BLH(xy, BLH);
        path.lat.push_back(BLH[0]);
        path.lon.push_back(BLH[1]);
    }
}
*/